#ifndef MATCH_KM_H
#define MATCH_KM_H

#include "common/algorithm_type/all_type.h"
// #include "common/log.h"
#include "match.h"
#include "match_center.h"
#include "rclcpp/rclcpp.hpp"
#include <algorithm>
#include <iostream>
#include <set>
#include <vector>

/*
km匹配算法：
       步骤0：创建一个大小nxm的矩阵，称为成本矩阵，其中矩阵中每个元素表示将n个工人分配给m个工作所需的成本。旋转矩阵，使其列数至少与行数一样多，并设k=min（n，m）。


       步骤1：对于矩阵的每一行，找到最小的元素，同时该行中的每个元素中减去这个最小元素。转至步骤2。


       步骤2：在得到的矩阵中找到一个零（Z）。如果行或列中没有标上星号的零，则把这个Z标上星号（以下称：star零）。对矩阵中的每个元素重复此操作。转至步骤3。


       第3步：用线覆盖包含一个star零的列。如果覆盖了K列，则这组star零表示一组完整的唯一赋值。在这种情况下，算法“完成”，否则，转到步骤4。


       第4步：找到一个未被覆盖的零，让这个零准备好（prime
it，以下将这些零称为：prime零）。


       如果包含prime零的行中没有star零，请转到第5步。否则，覆盖这一行，并揭开包含star零的列。以这种方式继续，直到矩阵中的零全部被覆盖为止。记录最小的未覆盖值，然后转到步骤6。


       第5步：按照如下方式构造一系列交替的prime零和star零。设Z0表示在步骤4中找到的未被覆盖的prime零。设Z1表示Z0列中的star零（如果有的话）。设Z2表示Z1行中的prime零（总会有一个）。


       继续这一过程（找到一个prime零，沿着它所在列找star零，再从找到的star零的所在行，找prime零），找到的是一个由star零和prime零交替组成的序列，该序列会终止于一个prime零，在它的列中没有star零。


       将这个序列中的star零全部取消星号，将序列的每个prime零标记为star零，将矩阵中所有的prime零变为普通零，并将整个矩阵取消覆盖。返回步骤3。


       步骤6：对于步骤4中找到的值，每个被覆盖行的每个元素加上该值，每个未覆盖列的每个元素中减去该值。在加减的过程中，跳过star零、prime零或者覆盖线。返回步骤4。


       完成：最终结果由成本矩阵中的star零位置表示。如果C（i，j）是星形零，工人i分配工作j。
*/

namespace perception {
namespace algorithm {
class MatchKM {
public:
  MatchKM(rclcpp::Node *private_nh);
  ~MatchKM();

  void Process(std::vector<LidarObject> &input_objs,
               std::vector<Match> &now_objs, float &euclidean_distance,
               std::vector<bool> &g_matched, std::vector<bool> &c_matched,
               std::vector<std::pair<int, int>> &pairs);
  void Process(std::vector<LidarObject> &input_objs,
               std::vector<MatchCenter> &now_objs, float &euclidean_distance,
               std::vector<bool> &g_matched, std::vector<bool> &c_matched,
               std::vector<std::pair<int, int>> &pairs);

private:
  std::vector<std::vector<int>>
  munkresAssignment(std::vector<std::vector<int>> &cost_matrix);
  bool star_in_row(int &row, int &col,
                   std::vector<std::vector<int>> &mask_matrix);
  void find_a_zero(int &row, int &col, int &step,
                   std::vector<std::vector<int>> &cost_matrix,
                   std::vector<std::vector<int>> &mask_matrix,
                   std::vector<int> &rowCover, std::vector<int> &colCover);
  void find_star_in_row(int &row, int &col,
                        std::vector<std::vector<int>> &mask_matrix);
  void find_star_in_col(int &row, int &col,
                        std::vector<std::vector<int>> &mask_matrix);
  void find_prime_in_row(int &row, int &col,
                         std::vector<std::vector<int>> &mask_matrix);
  void find_smallest(int &minval, std::vector<std::vector<int>> &cost_matrix,
                     std::vector<int> &rowCover, std::vector<int> &colCover);
  void augment_path(int &path_count, std::vector<std::vector<int>> path,
                    std::vector<std::vector<int>> &mask_matrix);
  void clear_covers(std::vector<int> &rowCover, std::vector<int> &colCover);
  void erase_primes(std::vector<std::vector<int>> &mask_matrix);
  void step_one(int &step, std::vector<std::vector<int>> &cost_matrix,
                std::vector<std::vector<int>> &mask_matrix);
  void step_two(int &step, std::vector<std::vector<int>> &cost_matrix,
                std::vector<std::vector<int>> &mask_matrix,
                std::vector<int> &rowCover, std::vector<int> &colCover);
  void step_three(int &step, std::vector<std::vector<int>> &cost_matrix,
                  std::vector<std::vector<int>> &mask_matrix,
                  std::vector<int> &rowCover, std::vector<int> &colCover);
  void step_four(int &step, std::vector<std::vector<int>> &cost_matrix,
                 std::vector<std::vector<int>> &mask_matrix,
                 std::vector<int> &rowCover, std::vector<int> &colCover,
                 std::vector<std::vector<int>> &path, int &path_row_0,
                 int &path_col_0);
  void step_five(int &step, std::vector<std::vector<int>> &cost_matrix,
                 std::vector<std::vector<int>> &mask_matrix,
                 std::vector<int> &rowCover, std::vector<int> &colCover,
                 std::vector<std::vector<int>> &path, int &path_row_0,
                 int &path_col_0);
  void step_six(int &step, std::vector<std::vector<int>> &cost_matrix,
                std::vector<int> &rowCover, std::vector<int> &colCover);
  void step_seven();

  void process_step(bool &done, int &step,
                    std::vector<std::vector<int>> cost_matrix_origin,
                    std::vector<std::vector<int>> &cost_matrix,
                    std::vector<std::vector<int>> &mask_matrix,
                    std::vector<int> &rowCover, std::vector<int> &colCover,
                    std::vector<std::vector<int>> &path, int &path_row_0,
                    int &path_col_0);
  void showCostMatrix(std::vector<std::vector<int>> &cost_matrix);
  void showMaskMatrix(std::vector<std::vector<int>> &mask_matrix);
  rclcpp::Node *private_nh_;
};
} // namespace algorithm
} // namespace perception
#endif